#! /usr/bin/env python3
# main_controller3.py
import RPi.GPIO as GPIO
import time
import threading
import cv2

# 导入电机控制模块
from motor_controller import init_gpio, start_speed_monitor, start_pwm_update_daemon, \
    set_motor_speed, rotate_in_place, drive_straight, drive_with_color, \
    stop_motor, cleanup as cleanup_motor

# 导入颜色检测模块
from detect_color import init_camera, start_color_detection, \
    get_latest_color_data, cleanup as cleanup_camera

# 导入超声波模块
from detect_distance import init_i2c, measure_distance, \
    start_distance_measurement, get_latest_distance, \
    cleanup as cleanup_distance

# ===== 可配置参数（修改此处无需改动函数） =====
# 1. 状态控制参数
COLOR_CONFIRM_COUNT = 3  # 需要连续识别相同颜色的次数
DISTANCE_THRESHOLD = 25.0  # 接近魔方的距离阈值(cm)
SEARCH_SPEED = 0.7  # 搜索魔方时的旋转速度
FORWARD_SPEED = 1.0  # 直行速度
TURN_SPEED = 0.8  # 转弯速度 #这个没有用到

# 2. 绕行参数
ARC_TIME = 1.5  # 每段圆弧行驶时间(秒)
OUTER_WHEEL_SPEED = 1.3  # 外侧轮速度
INNER_WHEEL_SPEED = 0.7  # 内侧轮速度
FINAL_SPRINT_SPEED = 1.5  # 最终冲刺速度

# 3. 颜色分类
LEFT_TURN_COLORS = ["red", "yellow"]  # 左转颜色
RIGHT_TURN_COLORS = ["blue", "green"]  # 右转颜色

# 4. 显示参数
DISPLAY_CAMERA = True  # 是否显示摄像头画面

# 状态管理类
class StateManager:
    def __init__(self):
        # 基本状态
        self.current_state = 1  # 当前状态(1,2,3)
        self.detected_color = None  # 当前检测到的颜色
        self.color_confirm_counter = {}  # 颜色确认计数器
        self.last_bypass_direction = None  # 上一次绕行方向('left'或'right')
        
        # 阶段完成标志
        self.state1_done = False
        self.state2_done = False
        self.state3_done = False
        
        # 计时器
        self.color_detection_time = None  # 颜色检测开始时间
        self.approach_time = None  # 接近魔方时间
        self.bypass_start_time = None  # 开始绕行时间
        self.arc_start_time = None  # 当前圆弧段开始时间
        self.current_arc = 0  # 当前圆弧段(1-4)
        
        # 绕行状态
        self.is_bypassing = False  # 是否正在绕行
        self.bypass_completed = False  # 是否完成绕行
        
        # 搜索状态
        self.search_direction = 'counterclockwise'  # 搜索方向
        self.search_start_time = None  # 搜索开始时间
        self.max_search_time = 3.0  # 最大搜索时间(秒)
        self.search_stage = 0  # 搜索阶段(0:左转, 1:右转)

    def set_state(self, new_state):
        """设置当前状态"""
        if self.current_state != new_state:
            self.current_state = new_state
            self.reset_state_variables()
            print(f"状态已更改为: {new_state}")

    def reset_state_variables(self):
        """重置状态相关变量"""
        self.detected_color = None
        self.color_confirm_counter = {}
        self.color_detection_time = None
        self.approach_time = None
        self.bypass_start_time = None
        self.arc_start_time = None
        self.current_arc = 0
        self.is_bypassing = False
        self.bypass_completed = False
        self.search_direction = 'counterclockwise'
        self.search_start_time = None
        self.search_stage = 0

    def determine_bypass_direction(self):
        """确定绕行方向"""
        if self.current_state == 1:
            # 状态1: 根据颜色决定
            if self.detected_color in LEFT_TURN_COLORS:
                direction = 'left'
            else:
                direction = 'right'
        elif self.current_state == 2:
            # 状态2: 与上一次相反
            if self.last_bypass_direction == 'left':
                direction = 'right'
            else:
                direction = 'left'
        else:  # 状态3
            # 状态3: 根据颜色决定
            if self.detected_color in LEFT_TURN_COLORS:
                direction = 'left'
            else:
                direction = 'right'
                
        self.last_bypass_direction = direction
        return direction

# 全局变量
running = True
state_manager = StateManager()
display_thread = None
global camera
# 显示摄像头画面的线程函数
def display_camera_thread():
    """显示摄像头实时画面的线程函数"""
    global running
    
    if not 'camera' in globals() or camera is None:
        print("错误: 摄像头未初始化，无法显示画面")
        return
    
    print("摄像头显示线程已启动")
    
    # 创建窗口
    cv2.namedWindow("摄像头画面", cv2.WINDOW_NORMAL)
    cv2.resizeWindow("摄像头画面", 640, 480)
    
    while running and DISPLAY_CAMERA:
        # 读取一帧图像
        ret, frame = camera.read()
        if not ret:
            print("警告: 无法从摄像头读取图像")
            time.sleep(0.1)
            continue
        
        # 获取最新的颜色检测结果，在画面上标记出来
        color_data = get_latest_color_data()
        
        # 在画面上标记检测到的颜色区域
        for color, segments in color_data.items():
            for segment in segments:
                x_start, x_end, x_center = segment
                # 计算y坐标（根据detect_color.py中的DEFAULT_ROW_PERCENT和DEFAULT_ROW_HEIGHT）
                height = frame.shape[0]
                row_percent = 0.5  # 默认值，与detect_color.py中保持一致
                row_height = 10    # 默认值，与detect_color.py中保持一致
                y_center = int(height * row_percent)
                
                # 在图像上绘制矩形标记颜色区域
                color_rgb = (0, 0, 255)  # 默认红色
                if color == "red":
                    color_rgb = (0, 0, 255)
                elif color == "green":
                    color_rgb = (0, 255, 0)
                elif color == "blue":
                    color_rgb = (255, 0, 0)
                elif color == "yellow":
                    color_rgb = (0, 255, 255)
            
                
                cv2.rectangle(frame, (x_start + frame.shape[1]//2, y_center-row_height//2), 
                             (x_end + frame.shape[1]//2, y_center+row_height//2), color_rgb, 2)
                cv2.putText(frame, color, (x_start + frame.shape[1]//2, y_center-15), 
                           cv2.FONT_HERSHEY_SIMPLEX, 0.5, color_rgb, 2)
        
        # 显示距离信息
        distance = get_latest_distance()
        cv2.putText(frame, f"距离: {distance:.1f} cm", (10, 30), 
                   cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255, 255, 255), 2)
        
        # 显示当前状态
        cv2.putText(frame, f"状态: {state_manager.current_state}", (10, 60), 
                   cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255, 255, 255), 2)
        
        # 显示检测到的颜色
        if state_manager.detected_color:
            cv2.putText(frame, f"颜色: {state_manager.detected_color}", (10, 90), 
                       cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255, 255, 255), 2)
        
        # 显示绕行方向
        if state_manager.last_bypass_direction:
            cv2.putText(frame, f"绕行: {state_manager.last_bypass_direction}", (10, 120), 
                       cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255, 255, 255), 2)
        
        # 显示图像
        cv2.imshow("摄像头画面", frame)
        
        # 按下q键退出
        if cv2.waitKey(1) & 0xFF == ord('q'):
            running = False
            break
        
        # 控制帧率
        time.sleep(0.03)  # 约30fps
    
    # 关闭窗口
    cv2.destroyAllWindows()
    print("摄像头显示线程已停止")

# 启动显示摄像头画面的线程
def start_display_camera():
    """启动显示摄像头画面的线程"""
    global display_thread
    
    if DISPLAY_CAMERA:
        display_thread = threading.Thread(target=display_camera_thread)
        display_thread.daemon = True  # 设为守护线程，主程序结束时自动结束
        display_thread.start()
        return display_thread
    return None

# 颜色检测函数
def detect_and_confirm_color(color_data):
    """
    检测并确认颜色
    
    Args:
        color_data: 颜色检测结果
    
    Returns:
        str: 确认的颜色，如果未确认则返回None
    """
    if not color_data:
        # 重置颜色计数器
        state_manager.color_confirm_counter = {}
        return None
    
    # 找到最宽的颜色块作为目标魔方
    widest_color = None
    max_width = 0
    
    for color, segments in color_data.items():
        if segments:  # 确保有检测到该颜色
            for segment in segments:  
                width = abs(segment[1] - segment[0])  # 计算宽度
                if width > max_width:
                    max_width = width
                    widest_color = color
    
    if widest_color:
        # 颜色确认逻辑
        if widest_color not in state_manager.color_confirm_counter:
            state_manager.color_confirm_counter[widest_color] = 1
        else:
            state_manager.color_confirm_counter[widest_color] += 1
        
        # 重置其他颜色的计数
        for color in list(state_manager.color_confirm_counter.keys()):
            if color != widest_color:
                state_manager.color_confirm_counter[color] = 0
        
        # 检查是否达到确认阈值
        if state_manager.color_confirm_counter.get(widest_color, 0) >= COLOR_CONFIRM_COUNT:
            return widest_color
    
    return None

# 执行绕行动作
def execute_bypass(direction):
    """
    执行绕行动作
    
    Args:
        direction: 绕行方向，'left'或'right'
    
    Returns:
        bool: 是否完成绕行
    """
    # 如果还没有开始绕行，初始化绕行参数
    if not state_manager.is_bypassing:
        state_manager.is_bypassing = True
        state_manager.bypass_start_time = time.time()
        state_manager.current_arc = 1
        state_manager.arc_start_time = time.time()
        print(f"开始{direction}侧绕行，第1段圆弧")
    
    # 计算当前圆弧段已经行驶的时间
    current_time = time.time()
    arc_elapsed_time = current_time - state_manager.arc_start_time
    
    # 如果当前圆弧段已经行驶足够时间，切换到下一段
    if arc_elapsed_time >= ARC_TIME:
        state_manager.current_arc += 1
        state_manager.arc_start_time = current_time
        print(f"切换到第{state_manager.current_arc}段圆弧")
        
        # 如果已经完成4段圆弧，绕行完成
        if state_manager.current_arc > 4:
            state_manager.is_bypassing = False
            state_manager.bypass_completed = True
            stop_motor()
            print("绕行完成")
            return True
    
    # 根据当前圆弧段和绕行方向设置电机速度
    if direction == 'left':
        if state_manager.current_arc == 1:
            # 第1段：左转弧
            set_motor_speed(INNER_WHEEL_SPEED, OUTER_WHEEL_SPEED)
        elif state_manager.current_arc == 2:
            # 第2段：右转弧
            set_motor_speed(OUTER_WHEEL_SPEED, INNER_WHEEL_SPEED)
        elif state_manager.current_arc == 3:
            # 第3段：右转弧
            set_motor_speed(OUTER_WHEEL_SPEED, INNER_WHEEL_SPEED)
        elif state_manager.current_arc == 4:
            # 第4段：左转弧
            set_motor_speed(INNER_WHEEL_SPEED, OUTER_WHEEL_SPEED)
    else:  # 右侧绕行
        if state_manager.current_arc == 1:
            # 第1段：右转弧
            set_motor_speed(OUTER_WHEEL_SPEED, INNER_WHEEL_SPEED)
        elif state_manager.current_arc == 2:
            # 第2段：左转弧
            set_motor_speed(INNER_WHEEL_SPEED, OUTER_WHEEL_SPEED)
        elif state_manager.current_arc == 3:
            # 第3段：左转弧
            set_motor_speed(INNER_WHEEL_SPEED, OUTER_WHEEL_SPEED)
        elif state_manager.current_arc == 4:
            # 第4段：右转弧
            set_motor_speed(OUTER_WHEEL_SPEED, INNER_WHEEL_SPEED)
    
    return False

# 搜索魔方
def search_for_cube():
    """
    搜索魔方
    
    Returns:
        bool: 是否找到魔方
    """
    # 如果还没有开始搜索，初始化搜索参数
    if state_manager.search_start_time is None:
        state_manager.search_start_time = time.time()
        state_manager.search_stage = 0
        print(f"开始搜索魔方，第1阶段：左转")
    
    current_time = time.time()
    search_elapsed_time = current_time - state_manager.search_start_time
    
    # 如果搜索时间过长，切换搜索方向
    if search_elapsed_time >= state_manager.max_search_time and state_manager.search_stage == 0:
        state_manager.search_stage = 1
        state_manager.search_start_time = current_time
        print(f"切换到第2阶段：右转")
    
    # 根据搜索阶段设置旋转方向
    if state_manager.search_stage == 0:
        # 第1阶段：左转
        rotate_in_place('counterclockwise', SEARCH_SPEED)
    else:
        # 第2阶段：右转
        rotate_in_place('clockwise', SEARCH_SPEED)
    
    return False

# 主控制函数
def main_control_loop():
    """主控制循环"""
    global running
    
    try:
        # 初始化电机
        init_gpio()
        print("电机已初始化")
        
        # 启动电机速度监测和PWM更新
        start_speed_monitor()
        start_pwm_update_daemon()
        print("电机速度监测和PWM更新已启动")
        
        # 初始化I2C设备（超声波）
        init_i2c()
        print("I2C设备已初始化")
        
        # 启动距离测量线程
        start_distance_measurement()
        print("距离测量线程已启动")
        
        # 初始化摄像头
        camera = init_camera()
        if camera is None:
            print("摄像头初始化失败，程序退出")
            return
        print("摄像头已初始化")
        
        # 启动颜色检测线程
        start_color_detection()
        print("颜色检测线程已启动")
        
        # 启动显示摄像头画面的线程
        if DISPLAY_CAMERA:
            start_display_camera()
            print("摄像头画面显示已启动")
        
        # 等待系统稳定
        print("系统初始化中，请稍候...")
        time.sleep(2)
        
        # 主控制循环
        while running:
            # 每0.1s执行一次处理
            time.sleep(0.1)

            # 获取最新的颜色检测结果
            color_data = get_latest_color_data()
            
            # 获取超声波距离
            distance = get_latest_distance()
            
            # 状态1：识别并通过第一个魔方
            if state_manager.current_state == 1:
                # 如果已经完成状态1，进入状态2
                if state_manager.state1_done:
                    state_manager.set_state(2)
                    continue
                
                # 如果正在绕行，继续执行绕行动作
                if state_manager.is_bypassing:
                    bypass_completed = execute_bypass(state_manager.last_bypass_direction)
                    if bypass_completed:
                        state_manager.state1_done = True
                    continue
                
                # 如果已经确认了颜色，直接使用该颜色进行导航
                if state_manager.detected_color:
                    # 使用颜色偏移量控制小车行驶
                    color_segments = color_data.get(state_manager.detected_color, [])
                    if color_segments:
                        # 使用最宽的颜色段的中心点作为导航目标
                        widest_segment = max(color_segments, key=lambda s: abs(s[1] - s[0]))
                        x_center = widest_segment[2]  # 获取中心点偏移量
                        drive_with_color(x_center, FORWARD_SPEED)
                    else:
                        # 如果看不到目标颜色，直行
                        drive_straight(FORWARD_SPEED)
                    
                    # 检查距离
                    if distance <= DISTANCE_THRESHOLD and distance > 0:
                        # 停车
                        stop_motor()
                        print(f"接近魔方，距离: {distance}cm，停车")
                        
                        # 确定绕行方向
                        bypass_direction = state_manager.determine_bypass_direction()
                        print(f"决定{bypass_direction}侧绕行")
                        
                        # 开始绕行
                        execute_bypass(bypass_direction)
                else:
                    # 还没有确认颜色，尝试检测
                    confirmed_color = detect_and_confirm_color(color_data)
                    if confirmed_color:
                        state_manager.detected_color = confirmed_color
                        print(f"确认魔方颜色: {confirmed_color}")
            
            # 状态2：识别并通过第二个魔方
            elif state_manager.current_state == 2:
                # 如果已经完成状态2，进入状态3
                if state_manager.state2_done:
                    state_manager.set_state(3)
                    continue
                
                # 如果正在绕行，继续执行绕行动作
                if state_manager.is_bypassing:
                    bypass_completed = execute_bypass(state_manager.last_bypass_direction)
                    if bypass_completed:
                        state_manager.state2_done = True
                    continue
                
                # 如果已经确认了颜色，直接使用该颜色进行导航
                
                if state_manager.detected_color:
                    # 使用颜色偏移量控制小车行驶
                    color_segments = color_data.get(state_manager.detected_color, [])
                    if color_segments:
                        # 使用最宽的颜色段的中心点作为导航目标
                        widest_segment = max(color_segments, key=lambda s: abs(s[1] - s[0]))
                        x_center = widest_segment[2]  # 获取中心点偏移量
                        drive_with_color(x_center, FORWARD_SPEED)
                    else:
                        # 如果看不到目标颜色，直行
                        drive_straight(FORWARD_SPEED)
                    
                    # 检查距离
                    # 检查距离
                    if distance <= DISTANCE_THRESHOLD and distance > 0:
                        # 停车
                        stop_motor()
                        print(f"接近魔方，距离: {distance}cm，停车")
                        
                        # 确定绕行方向（与状态1相反）
                        bypass_direction = state_manager.determine_bypass_direction()
                        print(f"决定{bypass_direction}侧绕行")
                        
                        # 开始绕行
                        execute_bypass(bypass_direction)
                else:
                    # 还没有确认颜色，搜索魔方
                    search_for_cube()
                    
                    # 尝试检测颜色
                    confirmed_color = detect_and_confirm_color(color_data)
                    if confirmed_color:
                        # 停止搜索
                        stop_motor()
                        state_manager.search_start_time = None
                        state_manager.detected_color = confirmed_color
                        print(f"确认魔方颜色: {confirmed_color}")
            
            # 状态3：识别并通过第三个魔方
            elif state_manager.current_state == 3:
                # 如果已经完成状态3，进入最终冲刺
                if state_manager.state3_done:
                    # 最终冲刺
                    drive_straight(FINAL_SPRINT_SPEED)
                    continue
                
                # 如果正在绕行，继续执行绕行动作
                if state_manager.is_bypassing:
                    bypass_completed = execute_bypass(state_manager.last_bypass_direction)
                    if bypass_completed:
                        state_manager.state3_done = True
                    continue
                
                # 如果已经确认了颜色，直接使用该颜色进行导航
                if state_manager.detected_color:
                    # 使用颜色偏移量控制小车行驶
                    color_segments = color_data.get(state_manager.detected_color, [])
                    if color_segments:
                        # 使用最宽的颜色段的中心点作为导航目标
                        widest_segment = max(color_segments, key=lambda s: abs(s[1] - s[0]))
                        x_center = widest_segment[2]  # 获取中心点偏移量
                        drive_with_color(x_center, FORWARD_SPEED)
                    else:
                        # 如果看不到目标颜色，直行
                        drive_straight(FORWARD_SPEED)
                    
                    # 检查距离
                    if distance <= DISTANCE_THRESHOLD and distance > 0:
                        # 停车
                        stop_motor()
                        print(f"接近魔方，距离: {distance}cm，停车")
                        
                        # 确定绕行方向
                        bypass_direction = state_manager.determine_bypass_direction()
                        print(f"决定{bypass_direction}侧绕行")
                        
                        # 开始绕行
                        execute_bypass(bypass_direction)
                else:
                    # 还没有确认颜色，搜索魔方
                    search_for_cube()
                    
                    # 尝试检测颜色
                    confirmed_color = detect_and_confirm_color(color_data)
                    if confirmed_color:
                        # 停止搜索
                        stop_motor()
                        state_manager.search_start_time = None
                        state_manager.detected_color = confirmed_color
                        print(f"确认魔方颜色: {confirmed_color}")
            
            # 短暂暂停，避免CPU占用过高
            time.sleep(0.05)
    
    except KeyboardInterrupt:
        print("\n程序被用户中断")
    except Exception as e:
        print(f"程序出错: {e}")
    finally:
        # 停止电机
        stop_motor()
        
        # 清理资源
        cleanup_motor()
        cleanup_camera()
        cleanup_distance()
        
        print("程序结束，资源已清理")

# 程序入口
if __name__ == "__main__":
    main_control_loop()